/**
 * @file ArmorDetect.cpp
 * @author enemy1205 (enemy1205@qq.com)
 * @brief 装甲板识别类
 * @date 2021-09-12
 *
 */
#include "ArmorDetect.h"

using namespace cv;
using namespace std;

/**
 * @brief 装甲板识别类构造函数
 * @param img 输入流图像
 */
ArmorDetect::ArmorDetect(Mat &img) {
    this->frame = &img;
    this->bin = Pretreatment(*frame, Other_param.init_color_mode, Other_param.init_detect_mode, A_param.threshold, 0);
    this->searchBlobs();
    if (!this->current_blobs.empty()) {
        this->matchArmors();
        this->deleteStickyArmor();
    }
}

/**
 * @brief 构造时即获取当前帧灯条
 */
void ArmorDetect::searchBlobs() {
    vector<vector<Point>> contours;
    findContours(this->bin, contours, RETR_EXTERNAL, CHAIN_APPROX_NONE);
    vector<RotatedRect> boundRect;
    boundRect.reserve(contours.size());//预留空间后方便下标索引
    for (size_t i = 0; i < contours.size(); i++) {
        if (contourArea(contours[i]) < L_param.light_min_area)continue;//去除小片区域噪斑
        boundRect[i] = fitEllipse(contours[i]);
        //Blob_ptr pass_value=new Light_blob(boundRect[i]);/*共享指针不可直接=new初始化*/
        Blob_ptr pass_value(new Light_blob(contours[i], boundRect[i]));/*可通过构造函数初始化*/
        //Blob_ptr pass_value= make_shared<Light_blob>(boundRect[i]);/*make_shared初始化最安全规范*/
        if (pass_value->isValid()) {
            this->current_blobs.push_back(pass_value);
        } else pass_value.reset();//释放未用到的共享指针
    }
}

/**
 * @brief 根据灯条匹配装甲板
 */
void ArmorDetect::matchArmors() {
    //先对灯条排序，以免后面再排
    sort(current_blobs.begin(), current_blobs.end(),
         [](const Blob_ptr &blob1_ptr, const Blob_ptr &blob2_ptr) -> bool {
             return blob1_ptr->getCenter().x < blob2_ptr->getCenter().x;
         });
    for (int i = 0; i < this->current_blobs.size() - 1; ++i) {
        //        if (blobs[i]->isMatched) continue;
        for (int j = i + 1; j < this->current_blobs.size(); ++j) {
            //            if (blobs[j]->isMatched) continue;
            Features_ptr pass_value = make_shared<Armor>(current_blobs[i], current_blobs[j]);
            if (!pass_value->isValid()) continue;
            else {
                pass_value->updateRestInfo();
                this->features.push_back(pass_value);
            }
        }
    }
}

/**
 * @brief 根据构造灯条删去粘连的装甲板
 */
void ArmorDetect::deleteStickyArmor() {
    for (auto i = this->features.begin(); i != this->features.end(); ++i) {
        for (auto j = i + 1; j != this->features.end(); ++j) {
            if ((*i)->getSon1_Features() == (*j)->getSon1_Features() ||
                (*i)->getSon1_Features() == (*j)->getSon2_Features() ||
                (*i)->getSon2_Features() == (*j)->getSon1_Features() ||
                (*i)->getSon2_Features() == (*j)->getSon2_Features()) {
                if ((*i)->getMatchError() > (*j)->getMatchError()) {
                    this->features.erase(i);
                    // 由于删除了一个元素，但 i 之后还需要 +1，因此这里先 -1
                    i--;
                    break;
                } else {
                    this->features.erase(j);
                    // 由于删除了一个元素，但 j 之后还需要 +1，因此这里先 -1
                    j--;
                }
            }
        }
    }
}

/**
 * @brief 绘制函数
 * @param predicted_center 由Tracker所得预测中心
 */
void ArmorDetect::draw(const cv::Point2f &predicted_center) {
    /*灯条绘制*/
    for (const auto &blob: this->current_blobs) {
        ellipse(*this->frame, *blob->getRect(), cv::Scalar(0, 120, 255));
    }
    /*装甲板绘制*/
    if (!this->features.empty())
        for (const auto &armor: this->features) {
            circle(*this->frame, armor->getCenter(), 4, cv::Scalar(125, 200, 10), 2);
            //                    ellipse(frame2, armor.rec(), Scalar(255));
            for (int j = 0; j < 4; ++j) {
                line(*this->frame, armor->getVertexes()[j], armor->getVertexes()[(j + 1) % 4],
                     cv::Scalar(255), 2,
                     8);
            }
        }
    /*预测装甲板绘制*/
    if (predicted_center != cv::Point2f(0, 0)) {
        circle(*this->frame, predicted_center, 2, cv::Scalar(126, 152, 252), 2);
    }
}